#include "motor_control.h"
// 速度测量相关变量
static int32_t last_encoder_A = 0;
static int32_t last_encoder_B = 0;
static float current_speed_A = 0;
static float current_speed_B = 0;
/**
 * @brief A轮速度PID控制
 * @param target_speed 目标速度
 */
void Speed_PID_Control_A(float target_speed)
{
    if (target_speed == 0.0f) {
        PWMA_setzkb(0); // 如果目标速度为0，直接停止A轮
        return;
    }
    // 1. 计算当前速度
    current_speed_A = ENA;
    
    // 2. 运行PID控制器（增量式）
    float pid_output = PID_Incremental_Calc(&PID_encoder_ASPD, target_speed, current_speed_A, 0);
    
    // 3. 输出限幅
    if (pid_output > 999.0f) pid_output = 999.0f;
    if (pid_output < -999.0f) pid_output = -999.0f;
    
    // 4. 设置PWM输出
    PWMA_setzkb((int16_t)pid_output);
    
    // 调试输出
    //uart0_printf("A - Target:%.1f, Speed:%.1f, PID:%.1f\n", target_speed, current_speed_A, pid_output);
}

/**
 * @brief B轮速度PID控制
 * @param target_speed 目标速度
 */
void Speed_PID_Control_B(float target_speed)
{
    if (target_speed == 0.0f) {
        PWMB_setzkb(0); // 如果目标速度为0，直接停止B轮
        return;
    }
    // 1. 计算当前速度
    current_speed_B = ENB;
    
    // 2. 运行PID控制器（增量式）
    float pid_output = PID_Incremental_Calc(&PID_encoder_BSPD, target_speed, current_speed_B, 0);
    
    // 3. 输出限幅
    if (pid_output > 999.0f) pid_output = 999.0f;
    if (pid_output < -999.0f) pid_output = -999.0f;
    
    // 4. 设置PWM输出
    PWMB_setzkb(pid_output);
    
    // 调试输出
    // uart0_printf("%.1f, %.1f,%.1f\n", target_speed, current_speed_B,pid_output);
}

/**
 * @brief 速度PID控制主函数（每10ms调用一次）
 */
void speed_PID_proc(void)
{
    // 设置目标速度（可以根据需要修改）
    float target_speed_A = PID_encoder_ASPD.SetValue;  // A轮目标速度
    float target_speed_B = PID_encoder_BSPD.SetValue;  // B轮目标速度
    
    // 执行速度控制
    Speed_PID_Control_A(target_speed_A);
    Speed_PID_Control_B(target_speed_B);
}

/**
 * @brief 设置A轮目标速度
 * @param speed 目标速度值
 */
void set_target_speed_A(float speed)
{
    PID_encoder_ASPD.SetValue = speed;
}

/**
 * @brief 设置B轮目标速度  
 * @param speed 目标速度值
 */
void set_target_speed_B(float speed)
{
    PID_encoder_BSPD.SetValue = speed;
}

/**
 * @brief 获取当前A轮速度
 * @return 当前A轮速度
 */
float get_current_speed_A(void)
{
    return current_speed_A;
}

/**
 * @brief 获取当前B轮速度
 * @return 当前B轮速度
 */
float get_current_speed_B(void)
{
    return current_speed_B;
}

/**
 * @brief PID参数调整函数（用于调试）
 */
void tune_pid_parameters_A(float kp, float ki, float kd)
{
    PID_encoder_ASPD.Kp = kp;
    PID_encoder_ASPD.Ki = ki;
    PID_encoder_ASPD.Kd = kd;
}

void tune_pid_parameters_B(float kp, float ki, float kd)
{
    PID_encoder_BSPD.Kp = kp;
    PID_encoder_BSPD.Ki = ki;
    PID_encoder_BSPD.Kd = kd;
}
